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On  the 


Computation  of  the  Ego-Motion  and  Distance 
to  Obstacles  for  a  Micro  Air  Vehicle  * 


R.  V.  Iyer 


Abstract 

In  this  paper,  we  have  considered  the  problem  of  velocity  and  range  estimation  for  an  UAV 
using  camera  and  the  knowledge  of  total  speed  through  a  GPS  device.  Together  with  [5],  we 
have  shown  that  this  problem  can  be  solved  using  a  reliability-based  motion  computation  and 
a  optimization  problem  that  is  well-posed.  If  the  velocity  in  the  body  frame  is  known,  then 
the  problem  results  in  a  straight-forward  solution.  For  the  more  complicated  case,  when  only 
the  total  speed  is  known,  we  assume  that  the  component  of  the  linear  velocity  along  the  axis  of 
the  camera  is  positive.  We  show  that  these  assumptions  form  minimal  additional  information 
required  to  solve  the  problem  of  ego-motion  and  range  estimation. 


1  Introduction 

In  this  paper,  we  have  considered  the  problem  of  velocity  and  range  estimation  for  an  UAV  using 
vision  based  techniques.  Such  a  problem  is  of  great  importance  to  Micro  Air  Vehicles  (MAVs) 
that  fly  at  low  enough  altitudes  so  that  GPS  geo-registration  errors  can  cause  them  to  fly  into 
obstacles.  Loss  of  GPS  during  flight  for  short  periods  of  time  could  also  result  in  loss  a  MAV.  For 
such  applications  such  as  search  and  classify  targets,  it  is  necessary  for  MAV’s  to  carry  an  onboard 
camera  that  streams  video  signals  to  a  stationary  receiver.  The  question  that  naturally  arises  is 
whether  the  video  data  can  help  the  MAV  navigate  in  the  presence  of  obstacles.  Recent  work  along 
these  lines  can  be  found  in  [9,  7,  8]. 

The  problem  can  be  broken  down  naturally  into  three  subproblems: 

1.  Estimation  of  motion  flow  field  in  the  image  plane  of  the  camera  in  an  unconstrained  envi¬ 
ronment  (that  is,  the  camera  is  not  made  to  move  in  a  controlled  manner); 

2.  Estimation  of  the  linear  and  angular  velocities  of  the  MAV  and  the  current  range  of  the 
objects  in  the  visual  field;  and 

3.  Navigation  to  the  desired  way-points  while  avoiding  the  obstacles  that  might  cause  the  loss 
of  the  MAV. 

In  this  paper,  we  address  the  second  problem  listed  above.  The  fundamental  question  that  needs  to 
be  answered  for  this  sub-problem  is  whether  the  linear  and  angular  velocities  of  the  MAV  and  the 
current  range  of  the  objects  in  the  visual  field  of  an  MAV  can  be  computed  correctly,  assuming  that 
the  first  question  has  been  solved  correctly.  Assuming  that  component  of  the  linear  velocity  along 
the  axis  of  the  camera  is  positive,  we  show  here  that  subproblem  2  is  solvable  correctly,  provided 
additional  information  on  the  speed  of  the  MAV. 
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This  paper  is  to  be  studied  together  with  [5]  which  tackles  the  first  problem  of  motion  estimation 
using  a  reliability-based  correspondence  computation  scheme  that  applies  to  successive  frames  of  a 
video  stream.  Here  we  define  the  notion  of  a  set  of  nonsingular  structure  blocks  that  is  necessary 
for  the  solution  of  the  ego-motion  problem.  Other  techniques  for  the  computation  of  the  flow  field 
on  the  image  plane  include  the  optical  flow  [4,  10,  7]  and  scale-invariant  feature  tracking  methods 
(see  [6]  and  references  therein).  As  the  optical  flow  computation  can  result  in  wildly  inaccurate 
solutions  (see  [4]  for  a  discussion),  and  in  light  of  Theorem  3.2  it  seems  that  a  feature  tracking 
method  in  some  sense  is  necessary.  As  there  can  be  translation,  rotation  and  scaling  of  the  image 
from  one  frame  to  the  next,  it  is  clear  that  a  scale-invariant  approach  will  be  more  fruitful.  The 
theoretical  results  of  this  paper  do  not  depend  on  which  of  the  specific  motion-field  computation 
methods  are  used,  though  we  use  the  notation  and  terminology  employed  in  [5]. 

Polat  and  Pachter  [9]  while  considering  the  problem  of  INS  aiding  using  a  camera  came  to  a 
similar  conclusion  to  ours  of  the  necessity  of  knowing  the  speed  of  the  aircraft  to  determine  range. 
However,  their  analysis  was  essentially  one-dimensional,  while  we  study  the  full  three-dimensional 
case.  In  [7],  there  are  several  assumptions  made  that  are  stronger  than  that  required  in  this 
paper.  In  addition  to  assuming  the  direction  of  flight  to  be  along  a  coordinate  axis  of  a  camera- 
centered  coordinate  system,  they  also  assume  constant  altitude  and  assume  a  priori  knowledge  of 
the  probability  distribution  for  the  altitude  function.  They  also  assume  a  certain  smoothness  in 
the  depth  function  from  one  frame  to  the  next.  This  makes  the  flight  as  well  as  the  scenery  fairly 
constrained,  though  it  must  be  mentioned  that  the  authors  of  [7]  are  trying  to  solve  a  different 
problem  of  estimating  the  height  above  ground.  Here  we  are  interested  in  finding  what  minimal 
additional  information  is  required  during  unconstrained  flight  for  the  computation  of  ego-motion 
and  range. 

In  Section  2  -  3,  we  derive  the  basic  equations  of  camera  motion  using  general  notation  that 
is  used  in  basic  aerodynamics  texts  such  as  Etkin  [3].  The  advantage  of  this  is  that  we  can  work 
in  mixed  coordinates  for  the  angular  velocity  and  linear  velocity  that  is  very  convenient.  For  the 
linear  velocity  we  use  the  body  axes  of  the  aircraft,  while  for  the  angular  velocity  we  use  a  camera- 
centered  coordinate  frame.  We  also  allow  for  the  camera  to  be  pointed  in  a  general  direction  on 
the  aircraft,  and  for  the  possibility  of  it  being  gimballed.  We  believe  that  the  simplicity  of  our 
modeling  procedure  compares  favorably  to  other  approaches  [2]  is  of  interest  on  its  own.  Section 
3  contains  the  main  results  of  the  paper.  One  key  result  is  Theorem  3.3  that  gives  necessary  and 
sufficient  conditions  for  the  solution  of  the  ego-motion  problem. 


2  Kinematics  of  Camera  Motion 

There  are  several  reference  frames  employed  in  air  vehicle  computations.  The  main  ones  are  the 
inertial  frame  and  the  body  frame  which  is  centered  on  the  center  of  mass.  When  a  camera  is  used 
on  a  UAV,  one  introduces  an  additional  frame  that  is  centered  on  the  focus  of  the  camera.  If  the 
camera  is  fixed,  then  the  change  of  coordinates  from  the  body  frame  to  the  camera-centered  frame 
is  accomplished  by  a  fixed  rotation  and  translation.  This  is  the  case  we  assume  in  this  note. 

Figure  1  shows  an  inertial  frame  with  origin  at  the  point  O^,  an  UAV  body  frame  with  origin 
at  the  point  O5  and  the  camera-centered  frame  origin  at  the  point  Oc.  For  the  inertial  and  UAV 
body  frames  the  standard  convention  for  labelling  the  axes  is  assumed  [3]  with  X 5  pointing  through 
the  nose  of  the  aircraft,  I5  pointing  out  the  right  wing,  and  Z 5  pointing  down.  For  the  camera 
frame,  the  standard  convention  used  in  the  machine  vision  literature  is  assumed  [4]  with  Zc  pointing 
normal  to  the  image  plane.  Thus  if  the  camera  is  mounted  in  front  of  the  aircraft  with  the  image 
plane  parallel  to  the  I5  —  Z 5  plane  of  the  aircraft,  then  X5  and  Zc  will  be  parallel  and  perhaps 
collinear. 
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Figure  1:  Kinematics  of  Camera  Motion 


The  point  O5  has  coordinates  denoted  by  bi  in  the  inertial  frame.  The  point  Oc  has  coordinates 
denoted  by  bb  in  the  body  frame.  Suppose  that  a  point  P  in  space  has  the  inertial  coordinates 
Ri ,  body  coordinates  R 5  and  camera-cnetered  coordinates  Rc,  then  the  relation  between  these 
coordinates  are  given  by: 


Ri  —  Qib  Rb  +  bi,  (1) 

Rb  =  Qbc  Rc  +  h  (2) 

where  Q Qbc  E  SO (3)  are  3x3  matrices  satisfying: 

QfbQib  =  and  det(Qjfc)  =  1  (3) 

QbcQbc  =  and  det(Qftc)  =  1  (4) 


In  the  following,  all  variables  are  assumed  to  be  functions  of  time  unless  explicitly  stated  as  con¬ 
stants.  Equations  (1-2  )  immediately  imply  the  well-known  equations: 


Qib  —  Qib&bi  (5) 

Qbc  Qbc  (b) 

where  and  (lc  are  skew-symmetric  angular  velocity  matrices,  with  the  subscript  indicating  the 
frame  where  it  is  defined.  Differentiating  Equations  (1-2  )  with  the  condition  that  the  point  P  is 
fixed  in  the  inertial  frame,  we  get: 


o  —  Qib  Rb  +  Qib  Rb  +  bi  =>  Rb  —  -(Qib  Qib  Rb  +  Qib  bi)  —  ~(&b  Rb  +  H)  (?) 

Rb  —  Qbc  Rc  +  Qbc  Rc  +  bjj  =>*  Rc  =  —(Qlc  Qbc  Rb  +  Qbc  bb)  —  Qbc  (&b  Rb  +  H) 

=  -  (Clc  Rc  +  Vc)  -  Ql  (Clb  Rb  +  Vb)  (8) 


where:  V&  =  Qlb  bi  is  the  linear  velocity  of  the  UAV  in  the  body  coordinates,  and  Vc  —  Qbc  b b  is  the 
linear  velocity  of  the  camera  in  the  camera-centered  coordinates.  If  =  [Qki  ^ k2  D ^]T  where 
k  —  b  or  c,  then: 


and  it  is  easy  to  check  that: 


0 

^ k3 

^k2 

^ k3 

0 

5 

~^k2 

^ kl 

0 

Rk  ~ 

-  x  R]%. 

(9) 
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Equations  (7-8)  model  the  kinematics  of  aircraft  and  camera  motion.  In  special  cases,  Equation 
(8)  can  be  simplified: 


1.  If  the  camera  is  fixed  on  the  aircraft,  then  bb  =  0  =>►  Vc  =  0  and  Qbc  =  0  =>  Qc  —  0.  This 
leads  to  Equation  (8)  being  modified  to: 

Rc  =  -Qbc  (fib  Rb  +  Vb). 

Substituting  for  Rb  from  Equation  (2),  we  get: 

Rc  =  -Qbc  Qbc  Rc  -  Qbc  h  -  Qbc  H  =  -Qbc  ^b  Rc  -  Qbc  &b  h  -  Qbc  Vb-  (10) 


Equation  (10)  relates  the  velocity  of  a  point  in  space  in  camera- fixed  coordinates  to  linear 
and  angular  velocities  of  the  UAV.  This  is  the  key  equation  that  will  be  used  in  Section  3. 
Notice  that  even  if  the  camera  is  positioned  so  that  the  normal  to  the  image  plane  points 
along  the  nose  of  the  UAV,  the  matrix  Qbc  is  given  by: 


Qbc  — 


0  0  1 

0  1  0 

-10  0 


(11) 


2.  In  case  the  camera  is  gimballed  and  it  is  possible  to  control  the  angular  rate  of  the  camera 
with  bb  fixed,  we  get: 

u  =  nc  (12) 

Rc  =  —u  Rc  —  Qbc^b  Rc  —  Qbc&bh  —  QbcVb-  (13) 


3  Computation  of  Motion  Parameters  from  Reliability  Indexed 
Motion  Field 


Figure  2:  Notation  for  the  Real  Motion  Field  Computation 

Figure  2  shows  the  notation  used  for  the  modeling  the  camera  motion.  The  focal  plane  contains 
the  focus  of  the  camera  and  is  parallel  to  the  image  plane.  The  origin  for  the  3  dimensional 
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coordinate  system  (X,  Y,  Z)  (henceforth  referred  to  as  the  camera-centered  coordinate  system)  is 
at  the  focus  of  the  camera.  As  is  customary  in  the  machine  vision  literature,  it  is  assumed  that  the 
Y  axis  is  normal  to  the  focal  plane.  The  origin  for  the  coordinate  system  (x,  y)  on  the  image  plane 
is  located  at  the  point  (0,  0,  — /)  in  (X,  Y,  Z)  coordinates,  where  /  is  focal  length  of  the  camera. 
The  point  R  with  coordinates  (X,  Y,  Z)  maps  to  the  point  (x,  y)  on  the  image  plane  with 

x  =  —  f  — - ,  and  y  =  —  f  — . 

J  Z  u  J  Z 

As  the  image  is  inverted,  we  will  consider  a  “normalized”  image  plane  located  at  (0,  0, 1)  and 
parallel  to  the  focal  plane. 

Suppose  the  point  Rc  =  (X,  Y,  Z)  maps  to  the  point  (x,  y)  on  the  normalized  image  plane. 
Then: 

X  Y 

x  —  — ,  and  y  —  — .  (14) 

Z  Z  v  7 

Equation  (9)  implies  that  if  the  camera  moves  with  linear  velocity  V  =  (Vx,Vy,Vz)  and  the 

angular  velocity  fi  =  (fix,  fiy,  fiz),  then  in  the  camera-centered  coordinate  system,  the  velocity  of 

the  point  Rc  is  given  by  Equation  (10):  - 


(X,  Y,  Y)  =  Qbc  flb  Rc  -  Qbc  flb  bb  -  Qbc  Vb. 


(15) 


Figure  3:  Object  motion  in  camera  frame. 

Let  Qbc  =  [Qbc i  Qbc 2  Qbc3]  where  we  have  explicitly  written  the  columns  of  Qbc.  Denote  the 
angular  velocity  flb  represented  in  the  camera-centered  frame  as  flc  =  Qbcfib  —  fiz]T- 

Then: 

X  XZ 
x  -  ~Z~^R 

=  —  (x  <  Qbcs ,  Vb>  -  <  Qbci,Vb  >)  +  fix  xy  -  Dy(l  +  x2)  +  fiz  y 

(^  Qbcl ,  fib^b  X  <C  Qbc3,  fib^b  ^) 

~  1  (x  <  Qbc3,Vb  >  -  <  Qbd,Vb  >)  +  &xxy  -  fiy(i  +x2)  +  ttzy  (16) 
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Y  YZ 
y  ~  Z  ~  ~~ZY^ 

=  —  (y  <  Qbc3,  Vb  >  -  <  Qbc2 ,  Vb  >)  +  flx(l  +  y2)  ~  fly  xy  -  Vtz  x 

(<  Qbch  &bbb  >  ~y  <  Qbc3i  ^b^b  >) 

~  —  (y  <  Qbcs,Vb  >  -  <  Qbc2 ,  Vb  >)  +  flx(i  +  y2)  -  fly  %y  -  flz^-  (17) 

The  approximations  would  be  correct  if  ^  «  [0  0  0]T. 

Let  Vb  —  [Vbi  Vb2  Vb3]T .  For  the  special  orientation  of  the  camera  given  by  (11),  we  get  (assuming 
Vbi  >  0  which  means  that  the  air  vehicle  has  a  positive  speed  along  the  nose  of  the  air  vehicle): 


y  = 


X 

12 

z  - 

Z2 

Vbl 

(X+V* 

z 

r + % 

Y 

Z  ~ 

z2 

Hi 

(v  Vb 2 

z 

V  Hi 

+  fix  x  y  —  fly  (l  +  x<2)  +  flz  y 


+  flx(l  +  y2)  -  fly  xy  -  f Izx. 


(18) 


(19) 


An  erroneous  form  of  these  equations  appear  in  [1].  The  correct  form  with  a  different  method  of 
proof  appears  in  [2]. 

We  can  make  some  observations  based  on  these  equations. 


1.  The  equations  for  x  and  y  are  linear  in  Vb  and  fl. 

2.  If  SI  =  0  then  x  =  \  (x  <  Qbc3,  Vb>  -  <  Qbci,Vb  >)  and  y  =  \  (y  <  Qbc3,  Vh>  -  <  Qbc2 ,  Vb  >) , 
which  means  that  the  point  (xo,yo)  =  (<^fdt>’  <^ffl4>)  's  invariant  for  the  instanta- 
neous  motion.  Note  that  it  is  possible  for  the  point  (xo,j/o)  to  lie  outside  the  boundary  of 
the  normalized  image  plane.  For  the  special  case  (11),  the  invariant  point  turns  out  to  be: 

(xo,yo)  =  (“i47>  Hr)  • 


Figure  4:  Motion  Flow  for  the  case  72  =  0. 
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3.  Another  observation  for  the  case  Q  =  0  is  that  at  the  pixel  (x,y)  the  motion  vector  (x,y)  is 
directed  radially  outward  from  the  point  (xo,yo)-  The  length  of  this  vector  is  given  by: 

\\(x,y)\\  =  -  —  -  || (x,y)  -  (x0,y0)||-  (20) 

Thus  the  length  of  the  motion  vector  is  product  of  the  distance  of  the  point  (x,  y)  from  (xo,  yo) 
and  the  quantity  <<^bcJ’Vb> . 

3.1  Motion  Parameter  Computation 

We  will  consider  two  different  methods  for  motion  computation.  The  time  interval  [0,T]  is  parti¬ 
tioned  into  0  =  To  <•••<  Tk  <•••<  Tjy  =  T.  One  assumes  that  linear  velocity  at  time  i  is 
known  in  the  body  frame  and  the  angular  velocity  value  at  time  T&_ i  is  computed  at  time  using 
the  images  at  times  T&_ i  and  T&.  In  the  second  method,  both  linear  and  angular  velocities  are 
computed  simultaneously  for  time  i.  As  discussed  in  [5],  the  image  is  partitioned  into  structure 
and  non-structure  blocks,  Bm,  1  <  m  <  P,  and  the  optical  motion  vector  (xm,?/m)  is  computed  for 
the  structure  blocks  located  at  (xm,  ym )  with  say,  m  —  1,  •  •  •  ,  M. 

3.1.1  Method  1 

If  we  had  information  about  the  velocity  V&,  then  the  problem  becomes  very  simple  and  leads  to 
an  optimization  problem  that  has  an  analytic  solution.  This  case  is  discussed  in  this  subsection. 
First,  we  eliminate  the  pixel-dependent  variable  Z  by  suitable  transformation.  A  similar  idea  (in 
slightly  different  contexts)  can  be  seen  in  [4]  and  [7]. 

Combining  Equations  (18)  and  (19)  and  getting  rid  of  the  Z,  we  have 


D\(x ,  y)x  -  D2(x,  y)y  =  C\{x,  y)Clx  +  Ci(x,  y)VlY  +  Cz{x ,  y)ttz-  (21) 

where 

Di(x,y)  =  (Hi  y~Vb2),  (22) 

D2{x,  y)  =  (Vblx  +  Vb3),  (23) 

Ci(x,y)  =  xy(Vbly -Vb2)  -  (l  +  y2)(Vblx +  Vb3),  (24) 

C2(x,  y)  =  —  (1  +  x2)(Vbi  y  —  Vb2)  +  xy(Vb\  x  +  Vb3),  (25) 

C3(x,y )  =  y{Vbiy-Vb2)  +  x(Vbix  +  Vb3).  (26) 


The  unknown  variables  ( Qx^y^z )  can  be  obtained  with  Least  Mean  Square  Error  (LMSE) 
fitting  weighted  by  the  reliability.  Let  (xm,7/m)  be  the  pixel  coordinate  of  the  center  of  block  Bm. 
From  the  motion  field  analysis,  we  have  obtained  the  motion  vector  (xm,  ym )  for  this  pixel  and  the 
associated  reliability  measure  ym.  The  weight  LMS  can  be  written  into  a  matrix  form  as 

rAfj  =  rb, 

where 

C'i(^1,y1)  C2{xl,yl) 

Ci(x2,y2)  C2(x2,y2) 

A  = 

_  Ci(xM,yM )  C2(xM ,  yM 

and 


C3{x1,y1) 

C3(x2,y2) 


C3(xM,y 


M  rt,M\ 


(27) 


(28) 


7 


(29) 


D^x1,  y1)^1  +  D2(x1,y1)y1 

71 

b  = 

Di(x2,  y2)x2  +  D2(x2,  y2)y2 

,  r  = 

D\{xm ,  yM)xM  +  D2(xM ,  yM)yM 

7M 

The  solution  is  given  by 

=  [(TAYiTA^iTAYiTb).  (30) 


3.1.2  Method  II 

In  this  method,  we  show  how  the  linear  and  angular  velocities  in  the  body  frame  can  be  computed 
using  Equation  21.  Expanding  this  equation  we  get  the  following: 


where: 


P(Q,  Vb)  =  a0  +  alx  x  +  aly  y  +  a2x  x 2  +  a2xy  xy  +  a2y  y2  =  0  (31) 

off*  =  -Vb3nx  +  Vb2nY  +  Vb3ym  +  Vb2xm-  m  =  l,---,M  (32) 

<  =  Vb3Qz  -  VblQx  +  vblym ;  m  =  1,  •  •  •  ,  M  (33) 

a?y  =  Vb2nz  -  VblQY  -  vblxm ;  m  =  1,  ■  ■  •  ,  M  (34) 

d2x  =  Vb2^Y  +  Vbl^Z  (35) 

d2xy  =  —Vb2^X  +  VbzSly  (36) 

a2y  =  —Vb3Qx  +  Vbi^z  (37) 


It  is  not  necessary  for  the  coefficients  of  the  polynomial  (31)  to  be  zero,  because  the  coefficients  of 
the  first  three  terms  change  with  (x,y). 

To  solve  for  the  motion  parameters  (Q*,  Vb ) ,  we  find  the  arguments  such  that: 

M 

(tt*,Vb)=  arg  min  J(Q,  Vb)  =  arg  min  ^  \^m  Pm{Vt,V)\2  (38) 

(Q,Vb)  (Q,  Vb)  m= i 

Hi>0;||H||=^  Vbl  >0;  HHII  =V 

Thus  we  are  faced  with  a  minimization  problem  with  constraint  V51  >  0,  and  ||I4||  =  V  >  0  which 
is  the  ground  speed  measured  using  a  Global  Positioning  System.  The  idea  is  to  make  \Pm(Q,  V5) | 
as  small  as  possible  weighted  by  the  reliability  of  ( xm,ym ). 

Let  us  now  consider  the  existence  of  solutions  for  this  optimization  problem.  The  function 
P(fl,  Vb)  can  be  written  as: 


m  Vb)  =  VbT  (a(x,  y)n  +  B(x,  y) 


—x 

-y 

to 

+ 

to 

"  -y  x  " 

II 

V. 

y 

-xy 

1  +  x2 

-y 

II 

y 

ST 

1 

0 

_  -(i  +  y2) 

xy 

X 

0 

1  _ 

The  matrix  A(x,  y)  eigenvalues: 

0,  i  ^1  +  x2  ±  y(l  +  x2)2  -4(1  +  ~x2)y2  -  4 y^j  . 


(39) 
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The  right  eigenvector  corresponding  to  the  0  eigenvalue  is  vr(x,y)  =  [x  y  1]T,  while  the  left 
eigenvector  is  vi(x,y)  —  [—1  —  y  x\.  It  can  also  be  easily  checked  that  vi(x,y)  •  b(x,y,x,y)  —  0. 

The  physical  meaning  of  vf{x,y)  and  vr{x,y)  is  as  follows.  Substituting  these  vectors  for  V& 
and  Q  in  Equations  (18-19)  we  get  x  =  0  and  y  —  0.  Therefore  at  every  point  (x,y)  there  is  an 
ambiguity  in  the  estimation  of  the  linear  velocity  in  the  direction  vj  (x,  y)  and  the  angular  velocity 
in  the  direction  vi(x,  y).  However,  this  ambiguity  can  be  partially  resolved  by  knowledge  of  (xm,  ym ) 
at  several  points  m  =  1,  •  •  •  ,  M  where  M  >  6.  In  the  equation:  P(0,  V&)  =  0,  we  still  have  the 
issues  that  if  V&  ^  0  is  a  solution,  then  any  a  V&  is  a  solution  for  a/0.  This  is  resolved  by  the  two 
constraints  on  the  optimization  problem,  so  that  we  have  a  unique  solution. 

We  need  the  following  non-singularity  condition  for  the  structure  blocks. 

Definition  3.1  (Non-singularity  condition  for  structure  blocks)  The  set  of  structure  blocks 
together  with  the  estimated  motion  vectors  {((xm, ym),  (xm, ym), 7m);  m  —  1,  •••  ,  M}  where  the 
reliability  indices  ym  >  0,  are  said  to  form  a  non-singular  set  if  for  all  £  E  IR3  the  set  of  vectors: 

;  m  =  1,  •  •  •  ,  m\ 

contains  at  least  one  non-zero  vector. 


T  ±\A(xm,ym)Z  +  B(xm,ym) 


It  is  clear  that  this  is  a  necessary  condition  for  the  solution  of  the  problem,  because  otherwise 
J(fi,  Vs)  would  be  zero  for  some  spurious  value  of  angular  velocity. 


Lemma  3.1  Assume  that  the  axis  of  the  camera  is  pointed  along  the  nose  of  the  aircraft,  and  that 
the  distance  of  the  external  objects  from  the  camera  is  much  greater  than  the  distance  of  the  focus  of 
the  camera  from  the  center  of  mass  of  the  aircraft.  Then  the  true  velocities  (QbjrueiVbjrue)  satisfy 


A{x ,  y )  Qb,true  T  B{x,  y) 


%true 

Utrue 


-L  Vb. 


true 


for  all  (x,  y)  in  a  non-singular  set  of  structure  blocks. 


Proof:  The  first  assumption  is  necessary  so  that  we  can  use  Equations  (18  -  19).  Let  W  be  the  span 
of  set  T  in  IR3.  Let  Vb,true  =  VbdrUC  +  Vb,true ’  where  Vbftrue  is  the  component  of  Vh,true  perpendicular 

to  W  and  V^true  is  the  component  along  W.  We  can  pick  vectors  {Wf,  1<z</;1</<3}  that 
are  orthonormal,  and  span  W.  Then: 


b,true 


A(xm,  ym)  nb:true  +  B(xm,  yr‘ 


^ true 
VTrue 


JiS^truei  Vbftrue)  — 


2=1 

/ 

'EPi'Wi  and 

2=1 

M  /  I 

E  7”E“<^ 

m—  1  \  2=1  / 


Thus  J(Qtrue,Vb,true )  =  0  if  and  only  if  J2i=i  ai  =  0  f°r  which  is  true  if  and  only  if 

V^true  —  0.  This  proves  the  claim.  □ 

The  above  considerations  can  be  summed  up  in  the  following  theorem: 
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Theorem  3.1  Assume  that  the  axis  of  the  camera  is  pointed  along  the  nose  of  the  aircraft,  and  that 
the  distance  of  the  external  objects  from  the  camera  is  much  greater  than  the  distance  of  the  focus 
of  the  camera  from  the  center  of  mass  of  the  aircraft.  Suppose  that  the  true  total  speed  ||V^rue||  is 
known  at  any  instant  of  time  T&;  k  =  0,  •  •  •  ,  N.  Suppose  that  the  component  of  the  inertial  velocity 
along  the  X  axis  of  the  camera  in  Figure  2  is  positive,  that  is,  Vb\ jrue  >  0.  Furthermore,  suppose 
that  there  is  no  error  in  the  reliability-based  motion  analysis  with  the  number  of  structure  blocks 
M  >  6,  and  that  the  set  of  structure  blocks  form  a  non-singular  set.  Denote  the  true  velocities  of 
the  air  vehicle  by  ( Dtrue,Vb,true )•  Then,  there  exists  a  unique  solution  (£P,  Vjf)  to  the  optimization 
problem  (38),  and  this  solution  coincides  with  the  true  solution  (£^rue,  V^rne). 

Proof:  First  observe  that  the  cost  function  in  (38)  is  quadratic  as  a  function  of  (£1,  Vb)  and  that 
J (Dtruei  Vb,true )  =  0  for  the  following  reason.  We  substitute  {Fltruei  Vs, true)  into  Equations  (18  -  19) 
and  then  substitute  the  resulting  (xtrueiVtrue)  into  (39),  which  yields  P(£ltrue,Vb,true)  —  0.  This 
means  that  in  the  absence  of  noise,  the  algorithm  will  converge  to  a  point  in  the  equivalence  class 
{(£I,V&)  |  J(ft,V&)  =  0}.  We  need  to  show  that  this  equivalence  class  consists  of  only  one  point 
(fit  ruei  Vb,true)  • 

The  reason  for  M  >  6  is  that  there  are  6  parameters  to  be  estimated  and  so  we  need  at  least 
6  equations  for  the  structure  blocks.  Another  preliminary  observation  is  that  the  set  of  points 
{(£1,0);  £1  G  IR3}  lead  to  P(£l,  0)  =  0.  However,  these  points  are  eliminated  by  the  constraint 
||H||  =V>0. 

If  (Dtrue,  Vs, true)  is  the  true  solution,  and  the  result  of  the  reliability-based  motion  estimation 
(see  [5])  is  error-free  (that  is,  (xm,ym);  m  =  1,  •  •  •  ,  M  exactly  satisfies  Equations  (18  -  19)),  then 
we  will  show  that  the  result  of  the  optimization  is  (£1*,  Vjf)  =  (Dtrue,  Vb,true)-  By  rewriting  (39)  we 
get: 

P(£l,  Vb)  =  ~  Vb  •  (yi  X  Vytrue  +  A(x,  y)  (£1  -  totrue))-  (40) 

dearly,  if  (£1,  Vb)  —  (£ltriie?  Vb,true)  then  P(£l,  Vb)  —  0.  Now  suppose  P(£l,  Vb)  —  0.  If  £1  7^  Dtrue 
then  the  second  term  inside  the  parentheses  in  (40)  is  non-zero  for  a  generic  point  (x,y).  It  is  also 
a  quadratic  function  of  (x,  y)  by  the  definition  of  A(x,  y).  The  first  term  inside  the  parentheses  is  a 
linear  function  of  (x,  y)  for  a  given  vector  Vbjrue-  Hence  for  a  generic  point  (x,  y)  the  term  inside  the 
parentheses  is  not  zero  and  is  a  quadratic  function  of  (x,y).  As  Vb  is  a  constant,  P(£l,  Vb)  cannot 
be  zero  for  a  generic  point  (x,y),  which  implies  that  our  assumption  of  £1  7^  Dtrue  is  false. 

Next  suppose  that  £1  =  Dtrue-  Then  we  have: 

P(£l,  Vjf)  ~  V5  •  V[  X  Vb^true-> 

which  is  zero  for  any  generic  point  (x,y)  if  and  only  if  Vb  =  GtVbjruei  where  a  G  IR.  Due  to  the 
constraint  || V5 1|  =  || Vbjrue\\,  we  must  have  Vb  =  ±Vbjrue-  Now  the  second  constraint  Vbli  >  0 
combined  with  the  given  condition  Vbii true  >  0  implies  that  Vb  =  Vb,true •  El 


3.1.3  Effect  on  Noise  on  Velocity  Estimation  for  Method  II 

Next,  let  us  examine  the  effect  of  noise  on  the  computation  of  (£1,  Vb).  As  observed  in  [5]  there  are 
several  sources  of  noise  that  affect  the  computation  of  (x,y).  As  the  reliability  for  structure 
block  m  is  a  measure  of  the  noise  in  the  computed  value  of  (x,  y),  we  consider  the  reliability  analysis 
to  result  in  random  variables  (u,v)  with  mean  (xtrue,ijtrue)  and  standard  deviation  am  1.  For  each 
m,  as  0  <  7m  <  1,  we  consider  am  to  be  a  continuously  differentiable,  monotone  decreasing  function 
defined  on  (0, 1]  with 

1.  lim  crm(7)  =  00; 

7^0 
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2.  crm(  1)  =  0  and 

da m(7) 

3.  lim  ,  P'7  \ \ 2  ^  for  some  C  >  0. 

7^0  (7))2  C 

The  reason  for  the  last  assumption  will  become  clear  shortly.  For  example,  am( 7)  =  tan  (|(1  —  ym)) 
satisfies  the  requirements  with  C  =  In  practice,  the  function  crm  could  be  defined  through  em¬ 
pirical  observations. 

For  the  m-th  structure  block,  denote  ( u 171  :vm)  =  {%™ue,y™ue)  +  Thus  is  a 

vector  random  variable  with  mean  (0,  0)  and  standard  deviation  am  I.  Then  we  have  the  following 
theorem: 


Theorem  3.2  Assume  that  the  axis  of  the  camera  is  pointed  along  the  nose  of  the  aircraft,  and 
that  the  distance  of  the  external  objects  from  the  camera  is  much  greater  than  the  distance  of  the 
focus  of  the  camera  from  the  center  of  mass  of  the  aircraft.  Suppose  that  Vsi,true  >  0.  Further 
suppose  that  the  result  of  the  reliability  analysis  [5]  at  time  instant  is  a  set  of  vector  random 
variables  ( u with  mean  (ip%ue,y™ue)  and  standard  deviation  a171 1,  where  am  is  related  to  the 

reliability  ym  according  to  conditions  1-3  given  above.  Denote  Pm  =  P(D,  Vf)  .  Then 

(xrn  ,yrn  ,urn  ,vrn) 

for  any  7m  G  (0, 1]: 

E[7mPm}2  <  {CVbl,truef  \\(xm,ym)  -  (x0,y0)\\2  (41) 


mm 

(RR) 


where  C  =  max  xam(x)  and  (xq,  yo)  =  (  —  7^ 

*e[0,l]  1  W  V  Vbl,tme  ’  Vbl, 


-  (_Yk 3  ,true  Vb2,true 

true 


)■ 


Proof:  The  theorem  follows  from  straight  forward  computations.  We  compute  P ^  to  be: 

2 


=  [A(x,y)n  +  B(x,y) 

+vb  ( A(x,y)Q  +  B(x,y) 


rp' 

^ true 

•m 

atrue 


^ true 

•m 

ytrue 


+  VbTB(x,y) 


-'X 


£y  j 


W  £y]  BT(x,y)Vb 


VbT  B(x,y) 


-'X 

~m 


(42) 


From  this,  it  is  clear  that: 


E[Pl}  =  \VbT  (A(xjy)n  +  B(x,y) 


^ true 

•m 

ytrue 


+  a2mVbT  B(x,y)BT(x,y)Vb. 


(43) 


Hence  for  any  y771  G  (0, 1]: 


min  E[yrnPm]2  <  E[7mPm(ntrue,  Vb+rue)]2  =  (7m)2  (0  +  a2m  VbTtrue  B(x,  y)BT(x,  y)VHrue)  . 

(pyA 

We  now  find  an  upper  bound  for  the  product  7mcrm(7m).  The  function  g{x)  =  xarn(x)  is  defined 
on  (0, 1]  by  the  definition  of  crm(-).  However,  it  can  be  seen  that  lim  g{x)  is  well  defined: 

x^O 

X  1 

lim  g(x)  =  lim  — j—  =  lim  —  —  (by  L’Hospital’s  Rule)  =  C. 

x^O  x^O  — x^O  \a  \XJ) 


arn(x) 


(crm  (a?))2 


Hence,  by  setting  g(0)  =  lim  g(x),  we  have  a  well  defined  continuous  function  defined  on  [0, 1],  that 

x^O 

lias  a  maximum  which  we  denote  by  C.  It  can  be  easily  checked  that: 

Vb.true  P(T)  2/)-®  y)  Vs,  true  =  {Ybl.trueU  ~  ^62,  true)  T  (Vbl,  true  x  T  Vb3,true )  • 
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Therefore  for  any  7771  G  (0, 1]: 

min  E[7mPm}2  <  C2  ((Hi ,trueym  -  vb2 )truef  +  (Vbl,truexm  +  VbUrue )2). 

(nVb) 

This  equation  leads  to  the  claim.  □ 

The  interesting  aspect  of  this  theorem  is  that:  if  (xm,ym)  =  (#0,2/0)  then  rnin^[7mPm]2  =  0 

(RH) 

no  matter  what  7771  is!  A  conclusion  is  that  points  near  the  invariant  point  for  linear  motion  (#o,  Vo) 
(which  is  of  course  unknown  a  priori  for  Method  II),  are  more  reliable  in  terms  of  the  computation 
of  the  angular  and  linear  velocities  even  in  the  presence  of  noise!  The  angular  velocity  does  not 
matter  in  this  computation.  Thus,  we  have  motivated  the  importance  of  knowing  the  linear  velocity 
V\)  that  was  the  content  of  Method  I.  Notice  also  the  term  in  the  right  hand  side  of  (41),  and  the 
term  in  the  right  hand  side  of  (20)! 

To  sum  up  Method  II: 

Theorem  3.3  Assume  that  the  axis  of  the  camera  is  pointed  along  the  nose  of  the  aircraft,  and 
that  the  distance  of  the  external  objects  from  the  camera  is  much  greater  than  the  distance  of  the 
focus  of  the  camera  from  the  center  of  mass  of  the  aircraft.  Suppose  It  is  known  that  V51  >  0  -  that 
is,  the  component  of  the  velocity  in  the  body  frame  along  the  nose  of  the  aircraft  is  positive,  and 
the  set  of  structure  blocks  form  a  nonsingular  set  with  M  >  6. 

Then  the  linear  and  angular  velocities  (14,  V\f)  computed  by  (38)  coincides  with  (fltrue,  Vb.true)  if  and 
only  if  (i)  the  total  V  is  known  at  any  instant  of  time  T&;  k  —  0,  •  •  •  ,  N .  (ii)  the  vectors  (i777,  ym)  for 
the  structure  blocks  (#m,2/m)  are  estimated  correctly  and  coincide  with  the  true  values  (x™ue,yt(ue)- 


Proof:  The  if  part  of  the  claim  follows  from  Theorem  3.1.  The  only  if  part  follows  from  the  proof 
of  Theorem  3.2,  where  it  can  be  seen  by  Equations  (42-43)  that  unless  am  =  0,  the  solution  of  the 
optimization  problem  will  not  coincide  with  ( Qtrue,Vb,true )  in  general.  □ 

3.2  Range  Estimation 

Once  the  camera  motion  (fi,  V5)  is  computed  through  either  of  the  Methods  I  or  II,  we  can  determine 
the  range  (or  depth)  Z  for  each  block  in  the  scene.  As  discussed  earlier  and  detailed  in  [5],  the 
image  is  partitioned  into  blocks,  B77,  1  <  n  <  P.  If  the  (x777,?/777);  ;m  =  1,  •  •  •  ,  M  are  the  motion 
vectors  computed  for  the  structure  block  ( xn,yn )  using  the  reliability  based  estimation  scheme  [5], 
then  the  range  Zm  for  these  blocks  can  be  determined  by  least  mean  squared  error  estimation: 

Zm  =  aigmm[xm-f(xn,yn,Z)}2  +  [ym-g(xn,yn,Z)}2]  m  =  1  •  •  •  ,  M,  (44) 

Zj 


where: 


f(x,y,Z )  =  -j(x+  ~p)  +  nxxy  -MyV  +  x2)  +£lzy,  (45) 

g(x,y,Z)  =  IZ- (y  -  h^)  +  nx (1  +  y2)  -  ttyxy  -  ttzx.  (46) 

^  Hi 

Let  A77  =  {(#^,2/j)|l  <  j  <  Ln}  be  the  top  candidate  motion  vectors  for  the  n-th  non-structure 
motion  block.  Recall  that  the  non-structure  blocks  are  not  used  in  the  computation  of  (fi,  Vf)  and 
hence  we  may  have  multiple  vectors  for  a  non-structure  block.  If  the  block  that  corresponds  to  an 
object  in  the  scene  is  stationary,  the  true  motion  vector  must  satisfy  Eqs.  (18-19)  whose  right  hand 
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sides  are  defined  in  (45-46).  Observe  that  the  functions  /(x,  ?/,  •)  and  g(x,  y ,  •)  are  affine  functions  of 
^  for  each  x  and  y.  For  each  candidate  motion  vector  (i™,  y for  the  non-structure  block  (x71,  yn ), 
we  can  compute  the  corresponding  range  by  orthogonal  projection  (see  Figure  5): 

Z]  =  arg  min  [x?  -  f(xn,  yn,  Z)]2  +  [y?  -  g(xn ,  yn,  Z )]2.  (47) 

z 


The  corresponding  fitting  error  is  denoted  by 

En0  =  [x]  -  f(xn,  yn,  Z 7)]2  +  \j%  -  g(xn,  y\  Z?)]2.  (48) 

We  choose  the  motion  vector  in  the  collection  An  to  be  the  one  with  the  least  fitting  error: 

j*  =  arg  min  E 71 .  (49) 

3= i,-,£n 

The  range  of  the  block  is  given  by  and  the  associated  motion  vector  is  (i71* ,  y71* ) . 


4  Conclusion 

In  this  report,  we  have  considered  the  problem  of  velocity  and  range  estimation  for  an  UAV  using 
camera  and  the  knowledge  of  the  total  speed  of  the  UAV.  Together  with  [5],  we  have  shown  that 
the  ego-motion  problem  can  be  solved  using  a  reliability-based  motion  computation,  followed  the 
solution  of  a  well-posed  optimization  problem.  Once  the  velocities  have  been  found,  the  range  of 
the  objects  can  be  computed  easily. 
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